﻿/* Derived from CART.DLL, Cart Plugin for KSP.
Original Author: Tony Chernogovsky, SPb, Russia.
Previous Maintainer: Callum King-Underwood, England.
Current Maintainer: Andrea Cappelletti, Milan, Italy aka CleverWalrus.
License: CC-BY-SA, Attribution & Share-alike Creative Common Licence. 
Feel free to modify as long as:
a) original author is mentioned, and 
b) you distribute your code under the same license.*/

using System;
using System.Collections.Generic;
using UnityEngine;

// CartLight:
// A class storing a single light source. It's located at Transform's 
// origin and oriented along Transform's Y (green) axis. A surface of 
// Transform's object is lit with Self-Illuminating/Diffuse shader 
// when the light is turned on.
namespace CleverBobCat.WheeledVehicle
{
    public class CartLight
    {

        private static Shader lightShader = Shader.Find("Self-Illumin/Specular"),
                              metalShader = Shader.Find("KSP/Specular"),
                              bumpShader = Shader.Find("KSP/Bumped Specular");

        private Transform mParent;
        private Light mLight;
        private bool bumped = false;

        public CartLight(Transform t)
        {
            mParent = t;
            mLight = mParent.gameObject.AddComponent<Light>();
            mLight.transform.parent = mParent;
            mLight.type = LightType.Spot;
            mLight.renderMode = LightRenderMode.ForcePixel;
            mLight.shadows = LightShadows.None; // Deferred Lighting not 
            // supported in KSP.

            mLight.cullingMask = 0x7FFFFFFF & ~0x10000; // everything except
            // for layer 16 (i.e. 
            // crew faces) 
            mLight.enabled = false;

            bumped = t.renderer != null && t.renderer.material != null &&
                     t.renderer.material.GetTexture("_BumpMap") != null;

            mParent.renderer.material.shader = bumped ? bumpShader : metalShader;
        }

        public Color color
        {
            get { return mLight.color; }
            set { mLight.color = value; }
        }

        public float range
        { // meters
            get { return mLight.range; }
            set { mLight.range = value; }
        }

        public float intensity
        { // 0..8
            get { return mLight.intensity; }
            set { mLight.intensity = value; }
        }

        public float spotAngle
        { // degrees
            get { return mLight.spotAngle; }
            set
            {
                if (value <= 0 || value >= 360)
                {
                    mLight.type = LightType.Point;
                }
                else
                {
                    mLight.spotAngle = value;
                    mLight.type = LightType.Spot;
                }
            }
        }

        public bool enabled
        {
            get { return mLight.enabled; }
            set
            {
                mLight.enabled = value;
                mParent.renderer.material.shader =
                       value ? lightShader : bumped ? bumpShader : metalShader;
                mParent.renderer.material.color = color;
            }
        }
    }

    // CartWheel and CartWheelList: 
    // Classes that incapsulate wheel physics and animation. 
    // CheckGroundContact method returns true if the wheel touches the ground 
    // (and updates wheel's position along Y axis). Setting steerAngle also 
    // animates wheel rotation.
    // Changing deployed member deploys/retracts a wheel.

    public class CartWheelList : List<CartWheel>
    {
        public float steerVisibility = 1;
        public float deployTime = 0.5F;
        public float forwardFriction = 1;
        public float sidewaysFriction = 1;

        private bool mCreated = false,
                     mDeployed = false,
                     mHasMotor = false,
                     mCanSteer = false;

        private Vector3 mDirection = Vector3.zero;

        // Call after all the wheels are added to the list.
        public void Initialize()
        {

            // First determine wheels' median point. 
            Vector3 origin = Vector3.zero;
            foreach (CartWheel w in this)
                origin += w.origin;
            if (Count > 0)
                origin /= Count;
            else
                origin = Vector3.zero;

            mDirection = Vector3.zero;

            // Initialize steering coeffs. Calculate each wheel's offest as 
            // related to median point and use that offset to limit a steering 
            // angle: make inner wheels deverted to less extent than outer 
            // ones).
            float offset, maxOffset = 0;
            foreach (CartWheel w in this)
            {
                offset = Mathf.Abs(Vector3.Dot(w.origin - origin, w.direction));
                if (offset > maxOffset)
                    maxOffset = offset;
                mDirection += w.direction;
            }

            mDirection.Normalize();

            if (maxOffset > 0)
                foreach (CartWheel w in this)
                    w.SteerCoeff = Vector3.Dot(w.origin - origin, w.direction) / maxOffset;

            // Finally 'initialize' all the wheels, i.e. make them calculate
            // their radii and spring lengths.
            foreach (CartWheel w in this)
            {
                w.Initialize();
                if (w.canSteer)
                    mCanSteer = true;
                if (w.hasMotor)
                    mHasMotor = true;
            }

            foreach (CartWheel w in this)
                w.Deploy(mDeployed);
        }


        public void CreateWheels(bool deploy)
        {
            if (!mCreated)
            {
                foreach (CartWheel w in this)
                    w.CreateWheel(deploy);
                mCreated = Count > 0;
            }
        }

        public void DestroyWheels()
        {
            if (mCreated)
            {
                mCreated = false;
                foreach (CartWheel w in this)
                    w.DestroyWheel();
            }
        }

        public bool deployed
        {
            get { return mDeployed; }
            set
            {
                bool wheelExists = false,
                     wheelIntermediate = false;
                foreach (CartWheel w in this)
                {
                    if (w.created)
                        wheelExists = true;
                    if (w.intermediate)
                        wheelIntermediate = true;
                }

                if ((value != wheelExists) && !wheelIntermediate)
                {
                    mDeployed = value;
                    foreach (CartWheel w in this)
                        w.Deploy(mDeployed);
                }
            }
        }

        public bool wheelsCreated
        {
            get
            {
                foreach (CartWheel w in this)
                    if (w.created)
                        return true;
                return false;
            }
        }

        public Vector3 direction { get { return mDirection; } }
        public bool created { get { return mCreated; } }
        public bool hasMotor { get { return mHasMotor; } }
        public bool canSteer { get { return mCanSteer; } }

        public void FixedUpdate(float motorTorque, float brakingTorque,
                                 float steeringAngle, out bool grounded)
        {
            grounded = false;
            foreach (CartWheel w in this)
            {
                w.motorTorque = motorTorque;
                w.brakeTorque = brakingTorque;
                w.steerAngle = steeringAngle;
                if (w.CheckGroundContact())
                    grounded = true;
            }
        }
    }

    public class CartWheel
    {
        private CartWheelList mOwner;

        private Transform mWheel;
        private Transform mAnchor;
        private Transform mLever;
        private WheelCollider mCollider = null;
        private JointSpring mJoint;

        private bool mHasMotor;
        public bool hasMotor { get { return mHasMotor; } }
        private bool mCanSteer;
        public bool canSteer { get { return mCanSteer; } }

        private float mSteerAngle = 0;
        private float mMotorTorque = 0;
        private float mBrakeTorque = 0;

        private float mMass = 0.1F;

        private float mRadius = -1;
        private float mSpringLength = 0;

        private float mRotation = 0;
        public float SteerCoeff = 1;

        private Vector3 mRetractedPosition = Vector3.zero;
        private Vector3 mLeverOrigin = Vector3.zero;
        private float mLeverLength = 0;
        private bool mLeverRotating = false;

        private float mStartTime = 0;
        private bool mInitialized = false,
                     mCreated = false,
                     mDeploying = false,
                     mRetracting = false;

        private Vector3 eulerAngles = Vector3.zero;

        // Wheel origin and axes, in parent coordinate space
        private Vector3 mForward = Vector3.zero,
                        mUp = Vector3.zero,
                        mRight = Vector3.zero;

        private Quaternion mInitialRotation;

        // Make "whatVector" equal to "whatTarget" along "whatAxis".
        private static Vector3 SetCoord(Vector3 whatVector, Vector3 whatTarget, Vector3 whatAxis)
        {
            return whatVector + Vector3.Project(whatTarget - whatVector, whatAxis);
        }

        public CartWheel(CartWheelList owner,
                          Transform wheel, Transform anchor, Transform lever,
                          bool hasMotor, bool canSteer)
        {
            mOwner = owner;
            mWheel = wheel;
            mAnchor = anchor;
            mLever = lever;
            mHasMotor = hasMotor;
            mCanSteer = canSteer;
            UnityEngine.Object.Destroy(mAnchor.renderer);
            mJoint.targetPosition = 0;

            // Wheel operation axes, in parent coordinate space:
            mForward = mAnchor.parent.InverseTransformDirection(mAnchor.forward);
            mUp = mAnchor.parent.InverseTransformDirection(mAnchor.up);
            mRight = mAnchor.parent.InverseTransformDirection(mAnchor.right);

            // Position, to which wheel goes in 'retracted' state (it may be shifted
            // forward or backward, in relation to 'deployed' state -- see Rover).
            mRetractedPosition = mAnchor.localPosition;

            // ...and an anchor must be above the wheel. 
            mAnchor.localPosition = SetCoord(mRetractedPosition, mWheel.localPosition, mForward);

            if (mLever && mLever.name.StartsWith("rotating"))
            {
                mLeverRotating = true;
                mLeverOrigin = mLever.localPosition;
                mLeverLength = (mWheel.localPosition - mLeverOrigin).magnitude;
            }

            mInitialRotation = mWheel.localRotation;
        }

        public Vector3 origin { get { return mAnchor.localPosition; } }
        public Vector3 direction { get { return mForward; } }
        public bool created { get { return mCollider; } }
        public bool intermediate { get { return mDeploying || mRetracting; } }

        public void Initialize()
        {
            if (mRadius > 0) // already initialized
                return;

            // Spring length is the distance between Wheel and its Anchor.
            mSpringLength = Vector3.Dot(origin - mWheel.localPosition, mUp);

            // Determine wheel radius as the maximum distance along its local Y and Z
            // axes (X being the wheel's axle).
            Mesh m = mWheel.GetComponent<MeshFilter>().mesh;
            if (m)
            {
                float f;
                foreach (Vector3 v in m.vertices)
                {
                    f = v.y * v.y + v.z * v.z;
                    if (f > mRadius)
                        mRadius = f;
                }
                mRadius = Mathf.Sqrt(mRadius);
            }
            if (mRadius <= 0)
            {
                Debug.Log("CART: WARNING - " +
                           "failed to determine wheel radius");
                mRadius = 1;
            }

            mInitialized = true;
        }

        public float mass
        {
            get { return mMass; }
            set
            {
                if (value <= 0)
                {
                    Debug.Log("CART: WARNING - wheelMass must be positive");
                    return;
                }
                mMass = value;
                if (mCollider)
                    mCollider.mass = value;
            }
        }

        public float spring
        {
            get { return mJoint.spring; }
            set
            {
                mJoint.spring = value;
                if (mCollider)
                    mCollider.suspensionSpring = mJoint;
            }
        }

        public float damper
        {
            get { return mJoint.damper; }
            set
            {
                mJoint.damper = value;
                if (mCollider)
                    mCollider.suspensionSpring = mJoint;
            }
        }

        public float motorTorque
        {
            get { return mMotorTorque; }
            set
            {
                mMotorTorque = value;
                if (mCollider && mHasMotor)
                    mCollider.motorTorque = mMotorTorque;
            }
        }

        public float brakeTorque
        {
            get { return mBrakeTorque; }
            set
            {
                if (value < 0)
                {
                    Debug.Log("CART: WARNING - " +
                               "brakeTorque must be positive");
                    return;
                }
                mBrakeTorque = value;
                if (mCollider)
                    mCollider.brakeTorque = mBrakeTorque;
            }
        }

        public float steerAngle
        { // also animate wheel rotation.
            get { return mSteerAngle; }
            set
            {
                mSteerAngle = value;
                if (mCollider)
                {
                    mCollider.steerAngle = mCanSteer ? value * SteerCoeff : 0;

                    // Quite a tricky one: we rotate a wheel around anchor's axes,
                    // not giving a darn for parent transform or wheel's orientation.

                    // reset rotation...
                    mWheel.localRotation = mInitialRotation;

                    // anchor's axes in wheel's local space... 
                    Vector3 u = mWheel.InverseTransformDirection(mAnchor.up);
                    Vector3 r = mWheel.InverseTransformDirection(mAnchor.right);

                    mWheel.localRotation = mInitialRotation *
                                           Quaternion.AngleAxis(mOwner.steerVisibility * mCollider.steerAngle, u) *
                                           Quaternion.AngleAxis(mRotation, r);

                    mRotation += 6 * mCollider.rpm * TimeWarp.fixedDeltaTime;
                    while (mRotation > 360) mRotation -= 360;
                    while (mRotation < -360) mRotation += 360;
                }
            }
        }

        public void Deploy(bool value)
        {
            if (intermediate)
                return;
            if (value && !mCollider)
            {
                if (mCreated)
                    CreateWheel(true);
            }
            else if (!value)
            {
                if (!mCreated)
                    ResetPosition();
                if (mCollider)
                {
                    mRetracting = true;
                    mStartTime = Time.fixedTime;
                }
            }
        }

        private void RotateLever()
        {
            // rotating levers always look towards the corresponding wheel,
            // and _end_ at its origin point. I.e. rotate the lever, and then 
            // adjust its position extending/retracting the lever it from/to 
            // hull.

            Vector3 f = mWheel.localPosition - mLeverOrigin;
            bool up = Vector3.Dot(mLever.up, mAnchor.up) > 0;
            mLever.localRotation = Quaternion.LookRotation(
                f, up ? Vector3.up : -Vector3.up);
            mLever.localPosition = mWheel.localPosition -
                mLeverLength * f.normalized;
        }

        private void ResetPosition()
        {
            if (!mInitialized)
                return;

            mWheel.localPosition = SetCoord(mRetractedPosition, mWheel.localPosition, mRight);
            if (mLever)
                if (mLeverRotating)
                    RotateLever();
                else
                    mLever.localPosition = mWheel.localPosition;
        }

        public void CreateWheel(bool deploy)
        {
            mCreated = true;

            if (mCollider) // already created
                return;
            if (mRadius < 0) // not initialized
                return;

            if (deploy)
                ResetPosition();

            mCollider = mAnchor.gameObject.AddComponent<WheelCollider>();
            mCollider.radius = mRadius;
            mCollider.mass = mMass;
            mCollider.suspensionSpring = mJoint;
            mCollider.motorTorque = mHasMotor ? mMotorTorque : 0;
            mCollider.brakeTorque = mBrakeTorque;
            mCollider.steerAngle = mSteerAngle;

            WheelFrictionCurve c = mCollider.forwardFriction;
            c.stiffness = mOwner.forwardFriction;
            mCollider.forwardFriction = c;

            c = mCollider.sidewaysFriction;
            c.stiffness = mOwner.sidewaysFriction;
            mCollider.sidewaysFriction = c;

            if (deploy)
            {
                mCollider.suspensionDistance = 0; // start in upper position...
                mStartTime = Time.fixedTime; // ...and begin to deploy.
                mDeploying = true;
            }
            else
            {
                mCollider.suspensionDistance = mSpringLength;
            }
        }

        public void DestroyWheel()
        {
            mCreated = false;

            if (!mCollider)
                return;
            UnityEngine.Object.Destroy(mCollider);
            mCollider = null;
        }

        // Exponential interpolation with adjustable base.
        static private float Exprp(float t, float expBase)
        {
            if (expBase <= 1)
                throw new ArithmeticException("ExpBase must be greater than 1");
            t = Mathf.Clamp01(t) * 2;
            return t < 1 ?
                (0.5F - (expBase - Mathf.Pow(expBase, t)) / (expBase - 1) / 2) :
                (0.5F + (expBase - Mathf.Pow(expBase, 2 - t)) / (expBase - 1) / 2);
        }

        public bool CheckGroundContact()
        { // true if touching the ground
            bool result = false;
            if (!mCollider)
                return result;

            float offset = mCollider.suspensionDistance;
            WheelHit wh;
            if (mCollider.GetGroundHit(out wh))
            {
                wh.point = mAnchor.InverseTransformPoint(wh.point);
                offset = -(mCollider.radius + wh.point.y);
                if (offset < 0)
                    offset = 0;
                if (offset > mCollider.suspensionDistance)
                    offset = mCollider.suspensionDistance;
                result = true;
            }

            mWheel.localPosition = SetCoord(mWheel.localPosition, origin - offset * mUp, mUp);

            if (mDeploying || mRetracting)
            {
                float t = (Time.fixedTime - mStartTime) / mOwner.deployTime;
                t = Exprp(t, 7);
                if (mRetracting)
                    t = 1 - t;

                if (mDeploying && t >= 1)
                {
                    // wheel fully deployed.

                    t = 1;
                    mDeploying = false;

                }
                else if (mRetracting && t <= 0)
                {
                    // wheel's fully retracted.

                    mRetracting = false;
                    DestroyWheel();
                    mCreated = true;
                    ResetPosition();
                    return result;
                }

                mCollider.suspensionDistance = mSpringLength * t;
                mWheel.localPosition = SetCoord(mWheel.localPosition,
                                                 Vector3.Lerp(mRetractedPosition, origin, t), mForward);
            }

            if (mLever)
                if (mLeverRotating)
                    RotateLever();
                else
                    mLever.localPosition = mWheel.localPosition;

            return result;
        }
    }

    // As I don't know the proper way to read Color values from PART.CFG I'll
    // use Vector3 values instead. Here are the conversion methods:
    public class CFGColor
    {
        public static Vector3 Vector3(Color v)
        {
            return new Vector3(v.r, v.g, v.b);
        }
        public static Color Color(Vector3 v)
        {
            return new Color(v.x, v.y, v.z, 1);
        }
    }
}